import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import (DeclareLaunchArgument, GroupAction, LogInfo,
                            IncludeLaunchDescription, SetEnvironmentVariable)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node, PushRosNamespace

def generate_launch_description():
    # 获取参数
    use_sim_time = LaunchConfiguration('use_sim_time')

    # 打印参数
    print_use_sim_time = LogInfo(msg=['use_sim_time: ', use_sim_time])

    # 定义启动组
    nav_group = GroupAction([
        # 地图管理相关节点
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('maps_manager'), 'launch', "maps_manager.launch.py")),
            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
                                'global_parameter_file': LaunchConfiguration('global_parameter_file'),
                                'default_nav_parameter_file': LaunchConfiguration('default_nav_parameter_file'),
                                }.items()),
        # 感知模块相关节点
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('nav2_costmap_2d'), 'launch', "nav2_costmap_2d.launch.py")),
            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
                                'global_parameter_file': LaunchConfiguration('global_parameter_file'),
                                'default_robot_parameter_file': LaunchConfiguration('default_robot_parameter_file'),
                                'default_nav_parameter_file': LaunchConfiguration('default_nav_parameter_file'),
                                }.items()),
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('sensors_preprocess'), 'launch', "sensors_preprocess.launch.py")),
            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
                                'global_parameter_file': LaunchConfiguration('global_parameter_file'),
                                'default_robot_parameter_file': LaunchConfiguration('default_robot_parameter_file'),
                                }.items()),
        # 路径规划相关节点
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('path_planner'), 'launch', "path_planner.launch.py")),
            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
                                'global_parameter_file': LaunchConfiguration('global_parameter_file'),
                                'default_robot_parameter_file': LaunchConfiguration('default_robot_parameter_file'),
                                'default_nav_parameter_file': LaunchConfiguration('default_nav_parameter_file'),
                                }.items()),
        # # 导航控制相关节点
        # IncludeLaunchDescription(
        #     PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('motion_control_ros'), 'launch', "motion_control.launch.py")),
        #     launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
        #                         'global_parameter_file': LaunchConfiguration('global_parameter_file'),
        #                         'default_nav_parameter_file': LaunchConfiguration('default_nav_parameter_file'),
        #                         }.items()),
    ])

    ld = LaunchDescription([
        print_use_sim_time,
        nav_group
    ])
    return ld